/**
* @file mpu6050.h
* @brief Header file for the MPU6050 sensor module.
*
* This file contains the function declarations and macro definitions for interfacing with the MPU6050 sensor.
* It includes initialization functions, data reading functions, and utility functions for processing sensor data.
* It also defines the I2C pins and other related constants.
* @author Maker Studio
* @license Apache 2.0
* This code is open-sourced under the Apache 2.0 license. When using open-source libraries
* or code, please ensure to acknowledge the original authors and sources.
**/

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include "ohos_init.h"
#include <math.h>

#include "cmsis_os2.h"

#include "i2c.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"
#include "iot_i2c.h"
#include "iot_errno.h"

#include "cmsis_os2.h"

#define IOT_SUCCESS    0

#define M_PI            3.14159265358979323846

#define IOT_IO_NAME_GPIO_15 15
#define IOT_IO_NAME_GPIO_16 16

#include "iot_i2c.h"
#include "iot_errno.h"

#define AHT20_I2C_IDX 1
#define AHT20_STARTUP_TIME     20*1000
#define AHT20_CALIBRATION_TIME 40*1000
#define AHT20_MEASURE_TIME     75*1000
#define AHT20_DEVICE_ADDR   0x38
#define AHT20_CMD_CALIBRATION       0xBE
#define AHT20_CMD_CALIBRATION_ARG0  0x08
#define AHT20_CMD_CALIBRATION_ARG1  0x00
#define AHT20_CMD_TRIGGER       0xAC
#define AHT20_CMD_TRIGGER_ARG0  0x33
#define AHT20_CMD_TRIGGER_ARG1  0x00
#define AHT20_CMD_RESET      0xBA
#define AHT20_CMD_STATUS     0x71
#define AHT20_STATUS_BUSY_SHIFT 7
#define AHT20_STATUS_BUSY_MASK  (0x1<<AHT20_STATUS_BUSY_SHIFT)
#define AHT20_STATUS_BUSY(status) ((status & AHT20_STATUS_BUSY_MASK) >> AHT20_STATUS_BUSY_SHIFT)
#define AHT20_STATUS_MODE_SHIFT 5
#define AHT20_STATUS_MODE_MASK  (0x3<<AHT20_STATUS_MODE_SHIFT)
#define AHT20_STATUS_MODE(status) ((status & AHT20_STATUS_MODE_MASK) >> AHT20_STATUS_MODE_SHIFT)
#define AHT20_STATUS_CALI_SHIFT 3
#define AHT20_STATUS_CALI_MASK  (0x1<<AHT20_STATUS_CALI_SHIFT)
#define AHT20_STATUS_CALI(status) ((status & AHT20_STATUS_CALI_MASK) >> AHT20_STATUS_CALI_SHIFT)

#define	MPU6050_SMPLRT_DIV		0x19
#define	MPU6050_CONFIG			0x1A
#define	MPU6050_GYRO_CONFIG		0x1B
#define	MPU6050_ACCEL_CONFIG	0x1C

#define	MPU6050_ACCEL_XOUT_H	0x3B
#define	MPU6050_ACCEL_XOUT_L	0x3C
#define	MPU6050_ACCEL_YOUT_H	0x3D
#define	MPU6050_ACCEL_YOUT_L	0x3E
#define	MPU6050_ACCEL_ZOUT_H	0x3F
#define	MPU6050_ACCEL_ZOUT_L	0x40
#define	MPU6050_TEMP_OUT_H		0x41
#define	MPU6050_TEMP_OUT_L		0x42
#define	MPU6050_GYRO_XOUT_H		0x43
#define	MPU6050_GYRO_XOUT_L		0x44
#define	MPU6050_GYRO_YOUT_H		0x45
#define	MPU6050_GYRO_YOUT_L		0x46
#define	MPU6050_GYRO_ZOUT_H		0x47
#define	MPU6050_GYRO_ZOUT_L		0x48

#define	MPU6050_PWR_MGMT_1		0x6B
#define	MPU6050_PWR_MGMT_2		0x6C
#define	MPU6050_WHO_AM_I		0x75
#define MPU6050_ADDRESS_AD0_LOW     0x68
#define MPU6050_RA_CONFIG           0x1A
#define MPU6050_RA_ACCEL_CONFIG     0x1C
#define MPU6050_RA_FF_THR           0x1D
#define MPU6050_RA_FF_DUR           0x1E
#define MPU6050_RA_MOT_THR          0x1F
#define MPU6050_RA_MOT_DUR          0x20
#define MPU6050_RA_ZRMOT_THR        0x21
#define MPU6050_RA_ZRMOT_DUR        0x22
#define MPU6050_RA_FIFO_EN          0x23
#define MPU6050_RA_INT_PIN_CFG      0x37
#define MPU6050_RA_INT_ENABLE       0x38
#define MPU6050_RA_TEMP_OUT_H       0x41
#define MPU6050_RA_USER_CTRL        0x6A
#define MPU6050_RA_PWR_MGMT_1       0x6B
#define MPU6050_RA_WHO_AM_I         0x75

#define MPU6050_ADDRESS         0x68
#define CONFIG_I2C_MASTER_BUS_ID 1

#define I2C_MASTER_ADDR                   0x0
#define I2C_SLAVE_ADDR                    MPU6050_ADDRESS
#define I2C_SET_BAUDRATE                  400000
#define I2C_TASK_DURATION_MS              10
#define CONFIG_I2C_SCL_MASTER_PIN 15
#define CONFIG_I2C_SDA_MASTER_PIN 16
#define CONFIG_I2C_MASTER_PIN_MODE 2
#if defined(CONFIG_I2C_SUPPORT_INT) && (CONFIG_I2C_SUPPORT_INT == 1)
#define I2C_INT_TRANSFER_DELAY_MS         800
#endif

#define BUTTON_GPIO 13

#include <math.h>  

typedef struct {  
    float Q;
    float R;
    float P;
    float K;
    float X;
} kalman_filter_t;

typedef struct {
    float pitch;
    float roll;
    float yaw;
} angle_t;
